// Normalize to -180, 180
float normalize180(float input){
  if (input > 180.)    
    return input - 360.;
  else if(input < -180.)
    return input + 360.;
  else return input;
}


int PID_update(struct TCommande *paramsCommand, float position, float vitesse, struct TPID *paramsPID){
  float error = normalize180(paramsCommand->current - degrees(position));

  // P term
  paramsPID->Pterm =  paramsPID->Pgain*error;
  // I term
  // We should use a counter but we assume than the control loop is always at CONTROL_LOOP Hz. The precision is not very important here
  paramsPID->Iterm += paramsPID->Igain*constrain(error, -I_E_SATURATION, I_E_SATURATION)/CONTROL_LOOP; 
  paramsPID->Iterm = constrain(paramsPID->Iterm, -I_S_SATURATION, I_S_SATURATION);
  // D term
  paramsPID->Dterm = paramsPID->Dgain *(paramsCommand->diff*KD_COMMAND_PART - degrees(vitesse));

  // PID control
  paramsPID->control = paramsPID->Pterm + paramsPID->Iterm + paramsPID->Dterm;
  return constrain(paramsPID->control, -COMMAND_SATURATION, COMMAND_SATURATION);
}


// ROLL, PITCH and YAW PID controls
void Attitude_control(void){
  control_roll = PID_update(&command_roll, roll, Omega_Vector[0], &PIDroll);
  control_pitch = PID_update(&command_pitch, pitch, Omega_Vector[1], &PIDpitch);
  control_yaw = PID_update(&command_yaw, yaw, Omega_Vector[2], &PIDyaw);

  control_throttle = REVERSE_THROTTLE(signed(ch[CHANN_THROTTLE]) - MIN_CHANN)*MIX_THROTTLE;
}

// Mixing the commands and write commands to servos
void Mixing(void){
  if (AP_mode == MANUAL) { //manual mode
    PIDroll.Iterm = 0;  // reset I terms
    PIDpitch.Iterm = 0;
    PIDyaw.Iterm = 0;
    update_command(degrees(yaw), &command_yaw);  //voir pour initialiser la diff a zero ?

    //set all commands to RX commands
    control_roll = REVERSE_ROLL(signed(ch[CHANN_ROLL]) - NEUTRAL_CHANN)*MIX_ROLL;
    control_pitch = REVERSE_PITCH(signed(ch[CHANN_ELEVATOR]) - NEUTRAL_CHANN)*MIX_PITCH;
    control_yaw = REVERSE_YAW(signed(ch[CHANN_RUDDER]) - NEUTRAL_CHANN)*MIX_YAW;
    control_throttle = REVERSE_THROTTLE(signed(ch[CHANN_THROTTLE]) - MIN_CHANN)*MIX_THROTTLE;
  }
  else {//stabilization mode or test mode or radio failsafe mode
    if (ch[CHANN_THROTTLE] < (MIN_CHANN+40))  // Minimum throttle to start control
    {
      PIDroll.Iterm = 0;  // reset I terms
      PIDpitch.Iterm = 0;
      PIDyaw.Iterm = 0;
      // Initialize yaw command to actual yaw
      update_command(degrees(yaw), &command_yaw);  //voir pour initialiser la diff a zero ?

      // set all commands to zero
      control_roll = 0;
      control_pitch = 0;
      control_yaw = 0;
      control_throttle = 0;
    }
  } 

#if OUTPUT_MAESTRO == 1
  write4servos(SERVO_0_NEUTRAL + SERVO_0_COMMAND + TRIM_SERVO_0, SERVO_1_NEUTRAL + SERVO_1_COMMAND + TRIM_SERVO_1, SERVO_2_NEUTRAL + SERVO_2_COMMAND + TRIM_SERVO_2, SERVO_3_NEUTRAL + SERVO_3_COMMAND +TRIM_SERVO_3);
#endif

#if OUTPUT_SERVOTIMER2 == 1
 servo0.write(SERVO_0_NEUTRAL + SERVO_0_COMMAND + TRIM_SERVO_0);
 servo1.write(SERVO_1_NEUTRAL + SERVO_1_COMMAND + TRIM_SERVO_1);
 servo2.write(SERVO_2_NEUTRAL + SERVO_2_COMMAND + TRIM_SERVO_2);
 servo3.write(SERVO_3_NEUTRAL + SERVO_3_COMMAND + TRIM_SERVO_3);
#endif
}
